#include "power_control.h"
#include "tim.h"

/*运行变量*/

cap_t supercap = {0};                           //电容状况
float bat_voltage = 0;													//电池电压
enum Cap_State cap_state = CHARGE;              //控制板模式
enum Cap_State cap_state_pre = PROTECT;           

pos_pid_t pid_chg_power;    

#define GPIO_SOURSE       GPIOA
#define GPIO_CAP          GPIOB
#define GPIO_PIN_SOURSE   GPIO_PIN_15
#define GPIO_PIN_CAP      GPIO_PIN_3

uint16_t max_charge_power = 120.0f;

uint16_t num_ctrl = 0;
uint16_t flag_ctrl = 0;
uint16_t flag_protect = 0;


/*功能函数*/


/*
**************************************************
* 说明：float类型数值限幅
**************************************************
*/
float limiter(float min ,float max, float value)
{
		float value_temp = 0;
		if (value >= max) value_temp = max;
		else if(value <= min) value_temp = min;
		else value_temp = value;
		return value_temp;
}



/*
**************************************************
* 说明：ina226读取功率
**************************************************
*/
void get_Power()
{
	
		float power_temp = 0;
	
	  /*
	  电容电压每秒读100次
		输出功率每秒读300次（电控做功率控制）
		输入功率每秒读6000次（电容功率控制）
	  */
	
		if(flag_ctrl == 2)
		{
			power_temp = INA226_GetPower(INA226_ADDRESS1);										//电容充电输入功率
			supercap.charge_power = limiter(0.0f ,150.0f, power_temp);
			supercap.voltage = INA226_GetBusV(INA226_ADDRESS2);								//电容电压（充电时略高于电容电压）
			supercap.output_power = INA226_GetPower(INA226_ADDRESS2);					//底盘输出功率
		}
		
		else if(flag_ctrl == 3)
		{
			power_temp = INA226_GetPower(INA226_ADDRESS1);										//电容充电输入功率
			supercap.charge_power = limiter(0.0f ,150.0f, power_temp);
			supercap.output_power = INA226_GetPower(INA226_ADDRESS2);					//底盘输出功率
		}
		
		else if(flag_ctrl == 1)
		{
			power_temp = INA226_GetPower(INA226_ADDRESS1);										//电容充电输入功率
			supercap.charge_power = limiter(0.0f ,150.0f, power_temp);
		}
		
}



/*
**************************************************
* 说明：控制器初始化
**************************************************
*/
void controller_Init()
{
	
			/*确保开启时关闭输出*/
			HAL_GPIO_WritePin(GPIO_CAP, GPIO_PIN_CAP, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(GPIO_SOURSE, GPIO_PIN_SOURSE, GPIO_PIN_RESET);
			
		
			/*can过滤器初始化*/
			CAN1_FilterInit();
		
		
			/*INA226初始化*/
			INA226_init();
		
		
			/*开启调试串口接收，上车之后也可以通过蓝牙查看当前电容状况*/
			HAL_Delay(200);
			HAL_UART_Receive_DMA(&huart1,rx_buff,255);

		
			/*pid系数初始化*/
			position_Pid_Init(&pid_chg_power,2.5f,4.6f,0,150.0f,8260,30,38000);                    //恒充电功率输出


			/*打开hrtim与pwm输出*/
			hhrtim1.Instance->sTimerxRegs[0].PERxR = 40000;																		//通过修改重装载值PER，从而修改PWM的频率
			hhrtim1.Instance->sTimerxRegs[0].CMP1xR = 300;																		//通过修改比较值CMP，从而修改占空比
			HAL_HRTIM_WaveformCounterStart(&hhrtim1, HRTIM_TIMERID_TIMER_A); 									//开启子定时器A
			HAL_HRTIM_WaveformOutputStart(&hhrtim1, HRTIM_OUTPUT_TA1|HRTIM_OUTPUT_TA2); 			//通道打开
			
			
			/*开启定时器中断*/
			HAL_TIM_Base_Start_IT(&htim3);
}



/*
**************************************************
* 说明：模式切换
**************************************************
*/
void cap_State_Switch()
{
	
    if(supercap.voltage <= 9)
    {
        HAL_GPIO_WritePin(GPIO_CAP, GPIO_PIN_CAP, GPIO_PIN_RESET);
        HAL_Delay(3);
        HAL_GPIO_WritePin(GPIO_SOURSE, GPIO_PIN_SOURSE, GPIO_PIN_SET);
        cap_state = CHARGE;
        cap_state_pre = CHARGE;
    }

		
    if(cap_state != cap_state_pre && supercap.voltage >= 10)
    {
        switch (cap_state)    //三个状态的含义在power_control.h里面查看
        {
					
        case PROTECT:
					
            HAL_GPIO_WritePin(GPIO_CAP, GPIO_PIN_CAP, GPIO_PIN_RESET);
				    HAL_Delay(3);
					  HAL_GPIO_WritePin(GPIO_SOURSE, GPIO_PIN_SOURSE, GPIO_PIN_SET);
						HAL_HRTIM_WaveformOutputStop(&hhrtim1, HRTIM_OUTPUT_TA1|HRTIM_OUTPUT_TA2);
						HAL_HRTIM_WaveformCounterStop(&hhrtim1, HRTIM_TIMERID_TIMER_A);
						flag_protect = 1;
            break;
				
        case CHARGE:
					
            HAL_GPIO_WritePin(GPIO_CAP, GPIO_PIN_CAP, GPIO_PIN_RESET);
				    HAL_Delay(3);
					  HAL_GPIO_WritePin(GPIO_SOURSE, GPIO_PIN_SOURSE, GPIO_PIN_SET);
						flag_protect = 0;
            break;
						
        case DISCHARGE:
						
						HAL_GPIO_WritePin(GPIO_SOURSE, GPIO_PIN_SOURSE, GPIO_PIN_RESET);
				    HAL_Delay(3);
					  HAL_GPIO_WritePin(GPIO_CAP, GPIO_PIN_CAP, GPIO_PIN_SET);
						flag_protect = 0;
            break;
						
        default:
            break;
				
        }
        cap_state_pre = cap_state;
    }
    
}



/*
**************************************************
* 说明：can发送数据给电控
**************************************************
*/
uint8_t cap_candata[8];
void can_Send_Data()
{
		uint16_t power_output,power_charge,cap_voltage;
		
		power_output = (uint16_t)(supercap.output_power * 100);
		power_charge = (uint16_t)(supercap.charge_power * 100);
		cap_voltage  = (uint16_t)(supercap.voltage * 100);
	
		cap_candata[0] = power_output>>8;
		cap_candata[1] = power_output;
		cap_candata[2] = power_charge>>8;
		cap_candata[3] = power_charge;
		cap_candata[4] = cap_voltage>>8;
		cap_candata[5] = cap_voltage;
	  cap_candata[6] = cap_state;
	
	  CAN1_Transmit(0x211,cap_candata);
}



/*
**************************************************
* 说明：控制器运行
**************************************************
*/
void controllor_Run()
{
	
		if(flag_ctrl && flag_protect == 0)
		{
			
			/*读取功率*/
			get_Power();
			
			
			/*低压充电限制*/
			if(supercap.voltage <= 5.00f)
				pid_chg_power.target_val = 30.00f;

			
			/*恒功率输出pid*/
			if(fabs(supercap.charge_power - pid_chg_power.target_val) <= 0.3f)
			supercap.charge_power = pid_chg_power.target_val;
			hhrtim1.Instance->sTimerxRegs[0].CMP1xR = position_Pid_Realize(&pid_chg_power,supercap.charge_power);
		
			
			/*模式切换*/
			cap_State_Switch();
			
			
			/*串口发送*/
			uart_Data_Send();
			
			
			/*can发送*/
			if(flag_ctrl == 3)
				can_Send_Data();		//can发送频率300Hz
			
			
			/*清楚控制标志位*/
			flag_ctrl = 0;		//pid控制频率6000Hz，由ina226制约
			
		}
}



/*
**************************************************
* 说明：定时器中断回调，控制采样频率
**************************************************
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *hitm)
{
	if(hitm == &htim3)
	{
		num_ctrl++;
		num_ctrl%=6000;
		flag_ctrl = 1;
		
		if(num_ctrl%60 == 0)
			flag_ctrl = 2;
		else if(num_ctrl%20 == 0)
			flag_ctrl = 3;
	}
}
